//=====================================================================================================
// MahonyAHRS.c
//=====================================================================================================
//
// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date			Author			Notes
// 29/09/2011	SOH Madgwick    Initial release
// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
//
//=====================================================================================================

//---------------------------------------------------------------------------------------------------
// Header files

#include "MahonyAHRS.h"
#include <inttypes.h>                   // for Linux __int64 type used in fast inverse sqrt
#include <math.h>

//---------------------------------------------------------------------------------------------------
// Function declarations
//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
// double version: http://www.gamedev.net/topic/500229-optimized-invsqrt-double-/

double invSqrt(double x) 
{
  double halfx = 0.5 * x;
  double y = x;
  uint64_t i = *(uint64_t*)&y;
  i = 0x5fe6ec85e7de30da - (i>>1); // the magic number
  y = *(double*)&i;
  y *= (1.5 - (halfx * y * y));    // 1st iteration
  y *= (1.5 - (halfx * y * y));    // 2nd iteration
  return y;
}

//====================================================================================================
// Functions

MahonyAHRS::MahonyAHRS(double sampleT, double kp, double ki)
  : twoKp(2*kp),twoKi(2*ki),
    integralFBx(0.0),integralFBy(0.0),integralFBz(0.0),
    sample_period(sampleT),
    q0(1.0),q1(0.0),q2(0.0),q3(0.0)
{}

//---------------------------------------------------------------------------------------------------
// AHRS algorithm update

void 
MahonyAHRS::AHRSupdate(double gx, double gy, double gz, 
		       double ax, double ay, double az, 
		       double mx, double my, double mz) 
{
  double recipNorm;
  double q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;  
  double hx, hy, bx, bz;
  double halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
  double halfex, halfey, halfez;
  double qa, qb, qc;
  /*  
  // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
  if((mx == 0.0) && (my == 0.0) && (mz == 0.0)) {
    MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
    return;
  }
  */
  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if(!((ax == 0.0) && (ay == 0.0) && (az == 0.0)))
    {
      // Normalise accelerometer measurement
      recipNorm = invSqrt(ax * ax + ay * ay + az * az);
      ax *= recipNorm;
      ay *= recipNorm;
      az *= recipNorm;     
      
      // Normalise magnetometer measurement
      recipNorm = invSqrt(mx * mx + my * my + mz * mz);
      mx *= recipNorm;
      my *= recipNorm;
      mz *= recipNorm;   
      
      // Auxiliary variables to avoid repeated arithmetic
      q0q0 = q0 * q0;
      q0q1 = q0 * q1;
      q0q2 = q0 * q2;
      q0q3 = q0 * q3;
      q1q1 = q1 * q1;
      q1q2 = q1 * q2;
      q1q3 = q1 * q3;
      q2q2 = q2 * q2;
      q2q3 = q2 * q3;
      q3q3 = q3 * q3;   
      
      // Reference direction of Earth's magnetic field
      hx = 2.0 * (mx * (0.5 - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
      hy = 2.0 * (mx * (q1q2 + q0q3) + my * (0.5 - q1q1 - q3q3) + mz * (q2q3 - q0q1));
      bx = sqrt(hx * hx + hy * hy);
      bz = 2.0 * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5 - q1q1 - q2q2));
      
      // Estimated direction of gravity and magnetic field
      halfvx = q1q3 - q0q2;
      halfvy = q0q1 + q2q3;
      halfvz = q0q0 - 0.5 + q3q3;
      halfwx = bx * (0.5 - q2q2 - q3q3) + bz * (q1q3 - q0q2);
      halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
      halfwz = bx * (q0q2 + q1q3) + bz * (0.5 - q1q1 - q2q2);  
      
      // Error is sum of cross product between estimated direction and measured direction of field vectors
      halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
      halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
      halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
      
      // Compute and apply integral feedback if enabled
      if(twoKi > 0.0) {
	integralFBx += twoKi * halfex * sample_period;	// integral error scaled by Ki
	integralFBy += twoKi * halfey * sample_period;
	integralFBz += twoKi * halfez * sample_period;
	gx += integralFBx;	// apply integral feedback
	gy += integralFBy;
	gz += integralFBz;
      }
      else {
	integralFBx = 0.0;	// prevent integral windup
	integralFBy = 0.0;
	integralFBz = 0.0;
      }
    
      // Apply proportional feedback
      gx += twoKp * halfex;
      gy += twoKp * halfey;
      gz += twoKp * halfez;
    }
  
  // Integrate rate of change of quaternion
  gx *= (0.5 * sample_period);		// pre-multiply common factors
  gy *= (0.5 * sample_period);
  gz *= (0.5 * sample_period);
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx); 
  
  // Normalise quaternion
  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;
}

//---------------------------------------------------------------------------------------------------
// IMU algorithm update

void 
MahonyAHRS::AHRSupdateIMU(double gx, double gy, double gz, 
			  double ax, double ay, double az)
{
  double recipNorm;
  double halfvx, halfvy, halfvz;
  double halfex, halfey, halfez;
  double qa, qb, qc;
  
  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if(!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) 
    {
    
      // Normalise accelerometer measurement
      recipNorm = invSqrt(ax * ax + ay * ay + az * az);
      ax *= recipNorm;
      ay *= recipNorm;
      az *= recipNorm;        
      
      // Estimated direction of gravity and vector perpendicular to magnetic flux
      halfvx = q1 * q3 - q0 * q2;
      halfvy = q0 * q1 + q2 * q3;
      halfvz = q0 * q0 - 0.5 + q3 * q3;
      
      // Error is sum of cross product between estimated and measured direction of gravity
      halfex = (ay * halfvz - az * halfvy);
      halfey = (az * halfvx - ax * halfvz);
      halfez = (ax * halfvy - ay * halfvx);
      
      // Compute and apply integral feedback if enabled
      if(twoKi > 0.0) {
	integralFBx += twoKi * halfex * sample_period;	// integral error scaled by Ki
	integralFBy += twoKi * halfey * sample_period;
	integralFBz += twoKi * halfez * sample_period;
	gx += integralFBx;	// apply integral feedback
	gy += integralFBy;
	gz += integralFBz;
      }
      else {
	integralFBx = 0.0;	// prevent integral windup
	integralFBy = 0.0;
	integralFBz = 0.0;
      }
    
      // Apply proportional feedback
      gx += twoKp * halfex;
      gy += twoKp * halfey;
      gz += twoKp * halfez;
    
    } 
  // Integrate rate of change of quaternion
  gx *= (0.5 * sample_period);		// pre-multiply common factors
  gy *= (0.5 * sample_period);
  gz *= (0.5 * sample_period);
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx); 
  
  // Normalise quaternion
  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;
}

//====================================================================================================
// END OF CODE
//====================================================================================================
